package jp.tkym.labs.client.geom;

public class Coordinate3f {
	float px;
	float py;
	float pz;
	float alpha;
	float beta;
	float gamma;
	
	/**
	 * x[i] = R * x[i-1] + p;
	 * @param x
	 * @return
	 */
	Vector3f translate(Vector3f x){
		return x.multiply(rot()).add(pos());
	}
	
	/**
	 * x[i-1] = R^I * x[i] - R^I * p;
	 * @param x
	 * @return
	 */
	Vector3f inverseTranslate(Vector3f x){
		return x.multiply(invRot()).diff(pos().multiply(invRot()));
	}
	
	private Vector3f pos(){
		return new Vector3f(px, py, pz);
	}
	
	private Matrix3f invRot(){
		if(alpha == 0.0f && beta == 0.0f && gamma == 0.0f) 
			return new Matrix3f();
		return MatrixUtil.Euler.zxz(-gamma, -beta, -alpha);
	}
	
	private Matrix3f rot(){
		if(alpha == 0.0f && beta == 0.0f && gamma == 0.0f)
			return new Matrix3f();
		return MatrixUtil.Euler.zxz(alpha, beta, gamma);
	}
}